/*
 * @Author: _oufen
 * @Date: 2023-07-11 20:23:19
 * @LastEditTime: 2023-08-05 16:40:26
 * @Description:  主要控制代码
 */
/**
 *************************************************************************************************************************
 * @file    control.c
 * @author  oufen
 * @version V1.0
 * @date    2023-07-10
 * @brief   Control模块.c文件配置
 *************************************************************************************************************************
 * @attention
 *
 *
 *************************************************************************************************************************
 */

/* Includes -------------------------------------------------------------------------------------------------------------*/
#include "control.h"
#include "LED.h"

/* 定义 -----------------------------------------------------------------------------------------------------------------*/
Param_InitTypedef Param;
Flag_InitTypedef Flag;

#define MyABS(X) (((X) > 0) ? (X) : -(X)) // 绝对值函数

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 求出直线坐标  两点
// 并且求  两点之间直线上的点   上的坐标

// 求出dx
float get_dx(float x1, float x2, int n)
{
    return (x2 - x1) / n;
}

// 求出dy
float get_dy(float y1, float y2, int n)
{
    return (y2 - y1) / n;
}

// 给出 x1,x2  第几个坐标 两点间横坐标之差
float get_x(float x1, float x2, int N)
{
    // return x1 + (x2 - x1) / N;

    return x1 + N * get_dx(x1, x2, N);
}

// 给出 x1,x2  第几个坐标 两点间横坐标之差
float get_y(float y1, float y2, int N)
{
    // return x1 + (x2 - x1) / N;

    return y1 + N * get_dy(y1, y2, N);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**
 * @brief 主控制函数
 * @param  无
 * @retval  无
 **/
void maincontrol(void)
{
}

// uint8_t maincon_flag, maincon_flag2;
// uint8_t my_i1;

/**
 * @brief 	TIM6中断   10ms
 * @param 	无
 * @retval  无
 */
float speed1,
      speed2;

float X_pwm,
      Y_pwm;

extern u8 task_fahui;

// 左减右加   上一个
// 上减下加

u8 fahui_flag1;
void fahui_task11()
{
    static uint32_t myTime = 0; // 计时+5
    switch (fahui_flag1)
    {
    case 0:
        if (Red_X != 0 && Red_Y != 0 && Green_X != 0 && Green_Y != 0)
        {

            pid_servo_X.target_val = Green_X;
            pid_servo_Y.target_val = Green_Y;

            x_pwm += Servo_X_pid_control();
            y_pwm += Servo_Y_pid_control();

            // if ((Red_X >= pid_servo_X.target_val - 1) && (Red_X <= pid_servo_X.target_val + 1) && (Red_Y >= pid_servo_Y.target_val - 1) && (Red_Y <= pid_servo_Y.target_val + 1))
            // if ((Red_X >= pid_servo_X.target_val - 2) && (Red_X <= pid_servo_X.target_val + 2) && (Red_Y >= pid_servo_Y.target_val - 2) && (Red_Y <= pid_servo_Y.target_val + 2))
            // if ((Red_X -pid_servo_X.target_val >= 1) && (Red_X - pid_servo_X.target_val <= 1) && (Red_Y -pid_servo_Y.target_val >= 1) && (Red_Y - pid_servo_Y.target_val + 1))
            // if ((Red_X >= pid_servo_X.target_val - 1) && (Red_X <= pid_servo_X.target_val + 1) && (Red_Y >= pid_servo_Y.target_val - 1) && (Red_Y <= pid_servo_Y.target_val + 1))
            //            if ((Red_X - pid_servo_X.target_val >= -1) && (Red_X -pid_servo_X.target_val <= 1) && (Red_Y -pid_servo_Y.target_val >=- 1) && (Red_Y - pid_servo_Y.target_val <=1))
            //			{
            // if(pid_servo_X.err<=1 && pid_servo_X.err>=-1 && pid_servo_Y.err>=-1 && pid_servo_Y.err<=1)
            // if ((pid_servo_X.target_val >= Red_X - 6) && (pid_servo_X.target_val <= Red_X + 6) && (pid_servo_Y.target_val >= Red_Y - 6) && (pid_servo_Y.target_val <= Red_Y + 6))

            // if ((Red_X - pid_servo_X.target_val < 1) && (Red_X >= pid_servo_X.target_val + 1) && (Red_Y <= pid_servo_Y.target_val - 1) && (Red_Y >= pid_servo_Y.target_val + 1))

            if (MyABS(Red_X - Green_X) < 5 && MyABS(Red_Y - Green_Y) < 5)
            {
                // TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
                // TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
            }
        }
        break;
        //     break;
        // case 99:
        //     // Buzzer_IN = 0;
        //     // LED_BLUE = 1;
        //     TIM_Cmd(TIM4, DISABLE);
        //     break;
        // default:
        //     break;
    }
}

u8 fahui_flag2;
void fahui_task21()
{
    static uint32_t myTime = 0; // 计时
    switch (fahui_flag2)
    {
    case 0:
        if (Red_X != 0 && Red_Y != 0 && Green_X != 0 && Green_Y != 0)
        {

            pid_servo_X.target_val = Green_X;
            pid_servo_Y.target_val = Green_Y;

            x_pwm += Servo_X_pid_control();
            y_pwm += Servo_Y_pid_control();

            // if ((Red_X >= pid_servo_X.target_val - 1) && (Red_X <= pid_servo_X.target_val + 1) && (Red_Y >= pid_servo_Y.target_val - 1) && (Red_Y <= pid_servo_Y.target_val + 1))
            // if ((Red_X >= pid_servo_X.target_val - 2) && (Red_X <= pid_servo_X.target_val + 2) && (Red_Y >= pid_servo_Y.target_val - 2) && (Red_Y <= pid_servo_Y.target_val + 2))
            // if ((Red_X -pid_servo_X.target_val >= 1) && (Red_X - pid_servo_X.target_val <= 1) && (Red_Y -pid_servo_Y.target_val >= 1) && (Red_Y - pid_servo_Y.target_val + 1))
            // if ((Red_X >= pid_servo_X.target_val - 1) && (Red_X <= pid_servo_X.target_val + 1) && (Red_Y >= pid_servo_Y.target_val - 1) && (Red_Y <= pid_servo_Y.target_val + 1))
            //            if ((Red_X - pid_servo_X.target_val >= -1) && (Red_X -pid_servo_X.target_val <= 1) && (Red_Y -pid_servo_Y.target_val >=- 1) && (Red_Y - pid_servo_Y.target_val <=1))
            //			{
            // if(pid_servo_X.err<=1 && pid_servo_X.err>=-1 && pid_servo_Y.err>=-1 && pid_servo_Y.err<=1)
            if ((pid_servo_X.target_val >= Red_X - 4) && (pid_servo_X.target_val <= Red_X + 4) && (pid_servo_Y.target_val >= Red_Y - 4) && (pid_servo_Y.target_val <= Red_Y + 4))

            {
                // TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
                // TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);

                Buzzer_IN = 1;
                fahui_flag1 = 0;
            }
            else
            {
                Buzzer_IN = 0;
            }
            break;
        }
        break;
    case 1:
        Buzzer_IN = 0;

        //        if (myTime <) // 等待2s
        //        {
        //            myTime++;
        //            Buzzer_IN = 1;
        //			fahui_flag1 = 2;
        //        }
        //        else
        //        {
        //            myTime = 0;
        //            Buzzer_IN = 0;
        //            //fahui_flag1 = 2;
        //        }
        break;
    default:
        break;
    }
}

// 上下   上加下减
// 左右   左加右减
uint32_t count;
extern u8 my_task_yes;
void TIM6_IRQHandler(void) // 10ms
{
    if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
    {
        // LED_RED = !LED_RED; // 测试程序是否卡死
        TIM_ClearITPendingBit(TIM6, TIM_IT_Update);

//        if (task_fahui == 1)
//        {
//            //fahui_task11();
//        }

//        if (task_fahui == 2)
//        {
//            fahui_task21();
//        }

        // task4_control();

        // mainControl();

        // pid_servo_X.target_val = you_shang_x; // 这个就是这条直线 上的所有点
        // pid_servo_Y.target_val = you_shang_y;

        // x_pwm -= Servo_X_pid_control();
        // y_pwm += Servo_Y_pid_control();

        //  if(Red_X)
    }
}
/*****************************************************END OF FILE*********************************************************/
